﻿#include "algorithmpclpackage.h"

#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/filters/extract_indices.h>

#include <pcl/features/moment_of_inertia_estimation.h>

#include <iostream>
#include <pcl/console/parse.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/features/boundary.h>
#include <math.h>
#include <boost/make_shared.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/io/pcd_io.h>

#include <pcl/visualization/range_image_visualizer.h>
#include <pcl/features/normal_3d.h>

#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/covariance_sampling.h>
#include <pcl/filters/normal_space.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/boundary.h>
#include <pcl/io/ply_io.h>

#include<pcl/visualization/pcl_visualizer.h>

#define EPSILON 0.000001
#define PI 3.14159265358979323846264338

using namespace std;
using namespace cv;

int user_data;

AlgorithmPclPackage::AlgorithmPclPackage()
{}

//#include "stdafx.h"
//#include <iostream>
//#include <pcl/io/pcd_io.h>
//#include <pcl/io/ply_io.h>
//#include <pcl/point_cloud.h>
//#include <pcl/console/parse.h>
//#include <pcl/common/transforms.h>
//#include <pcl/visualization/pcl_visualizer.h>
///*输入输入点云，输出点云地址和分割平面方程，返回旋转完的点云，可直接xy面z轴0值分割*/
PointCloudT AlgorithmPclPackage::PlanRotate(PointCloudT input, double A, double B, double C, PointCloudT& transformed_cloud)
{
	//点云旋转，基于平面标准式法向量原理
	//先绕x旋转,再绕y旋转

	/* Reminder: how transformation matrices work :
	|-------> This column is the translation
	| 1 0 0 x |  \
	| 0 1 0 y |   }-> The identity 3x3 matrix (no rotation) on the left
	| 0 0 1 z |  /
	| 0 0 0 1 |    -> We do not use this line (and it has to stay 0,0,0,1)

	METHOD #1: Using a Matrix4f

	This is the "manual" method, perfect to understand but error prone !

	*/

	Eigen::Matrix4f transform_x = Eigen::Matrix4f::Identity();
	/* x转移矩阵
	  |1   0   0    |
	  |0  cos -sin  |
	  |0  sin  cos  |
	*/
	double cosx, sinx;
	double dis = sqrt(pow(A, 2) + pow(B, 2) + pow(C, 2));
	cosx = sqrt(pow(A, 2) + pow(C, 2)) / dis;
	sinx = B / dis;
	transform_x(1, 1) = cosx;
	transform_x(1, 2) = -sinx;
	transform_x(2, 1) = sinx;
	transform_x(2, 2) = cosx;

	Eigen::Matrix4f transform_y = Eigen::Matrix4f::Identity();
	/* y转移矩阵
	  |cos  0  sin  |
	  |0    1   0   |
	  |-sin 0  cos  |
	*/
	double cosy, siny;
	cosy = C / dis;
	siny = sqrt(pow(A, 2) + pow(B, 2)) / dis;
	// Define a rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
	transform_y(0, 0) = cosy;
	transform_y(0, 2) = siny;
	transform_y(2, 0) = -siny;
	transform_y(2, 2) = cosy;
	PointCloudT transformedx_cloud;
	pcl::transformPointCloud(input, transformedx_cloud, transform_x);
	pcl::transformPointCloud(transformedx_cloud, transformed_cloud, transform_y);
	return transformed_cloud;

}

//---------------------------------点云显示------------------------------------//
void AlgorithmPclPackage::viewerOneOff(pcl::visualization::PCLVisualizer& viewer)
{
	viewer.setBackgroundColor(1.0, 0.5, 1.0);//设置背景色
	pcl::PointXYZ o;
	o.x = 1.0;
	o.y = 0;
	o.z = 0;
	viewer.addSphere(o, 0.25, "sphere", 0);//设置范围（中心点、半径、）
	std::cout << "i only run once" << std::endl;
}

void AlgorithmPclPackage::viewerPsycho(pcl::visualization::PCLVisualizer& viewer)
{
	static unsigned count = 0;
	std::stringstream ss;
	ss << "Once per viewer loop: " << count++;
	viewer.removeShape("text", 0);//删除一个ID
	viewer.addText(ss.str(), 200, 300, "text", 0);//添加一个文本ID
	//FIXME: possible race condition here:
	user_data++;
}

//点云读取
int AlgorithmPclPackage::readCloudPLY(const std::string& name, pcl::PointCloud<pcl::PointXYZ>::Ptr* pCloudScene)
{
	/*(*pCloudScene)->clear();*/
	(*pCloudScene).reset(new PointCloudT);
	std::string PLYname = name;// +".ply";
	pcl::PLYReader reader;
	reader.read<PointT>(PLYname.c_str(), *(*pCloudScene));//加载点云
	return 0;
}

int AlgorithmPclPackage::readCloudPCD(const std::string& name, pcl::PointCloud<pcl::PointXYZ>::Ptr* pCloudScene)
{
	//(*pCloudScene)->clear();
	(*pCloudScene).reset(new PointCloudT);
	std::string PCDname = name;// +".ply";
	pcl::PLYReader reader;
	pcl::io::loadPCDFile(PCDname.c_str(), *(*pCloudScene));//加载点云
	return 0;
}

//点云读取
int AlgorithmPclPackage::readCloud(const std::string& name, pcl::PointCloud<pcl::PointXYZ>::Ptr* pCloudScene)
{
	/*(*pCloudScene)->clear();*/
	(*pCloudScene).reset(new PointCloudT);

	std::string suffix = name.substr(name.size() - 3, 3);
	int iRes = 0;
	if (suffix == "ply") {
		iRes = readCloudPLY(name, pCloudScene);
	} else if (suffix == "pcd") {
		iRes = readCloudPCD(name, pCloudScene);
	}

	return iRes;
}

int AlgorithmPclPackage::readCloudPLY(const std::string& name, pcl::PointCloud<pcl::PointXYZLNormal>::Ptr* pCloudScene)
{
	/*(*pCloudScene)->clear();*/
	(*pCloudScene).reset(new pcl::PointCloud<pcl::PointXYZLNormal>);
	std::string PLYname = name;// +".ply";
	pcl::PLYReader reader;
	reader.read<pcl::PointXYZLNormal>(PLYname.c_str(), *(*pCloudScene));//加载点云
	return 0;
}

int AlgorithmPclPackage::readCloudPCD(const std::string& name, pcl::PointCloud<pcl::PointXYZLNormal>::Ptr* pCloudScene)
{
	//(*pCloudScene)->clear();
	(*pCloudScene).reset(new pcl::PointCloud<pcl::PointXYZLNormal>);
	std::string PCDname = name;// +".ply";
	pcl::PLYReader reader;
	pcl::io::loadPCDFile(PCDname.c_str(), *(*pCloudScene));//加载点云
	return 0;
}

//点云读取
int AlgorithmPclPackage::readCloud(const std::string& name, pcl::PointCloud<pcl::PointXYZLNormal>::Ptr* pCloudScene)
{
	/*(*pCloudScene)->clear();*/
	(*pCloudScene).reset(new pcl::PointCloud<pcl::PointXYZLNormal>);

	std::string suffix = name.substr(name.size() - 3, 3);
	int iRes = 0;
	if (suffix == "ply") {
		iRes = readCloudPLY(name, pCloudScene);
	} else if (suffix == "pcd") {
		iRes = readCloudPCD(name, pCloudScene);
	}

	return iRes;
}

int AlgorithmPclPackage::saveCloudToPLY(const std::string& name, pcl::PointCloud<pcl::PointXYZ>::Ptr pCloudScene)//保存 点云.ply
{
	//pcl::PointCloud<pcl::PointXYZ>::Ptr pCloudScene;
	//pCloudScene.reset(new pcl::PointCloud<pcl::PointXYZ>);//点云
	//pCloudScene = m_pCloud;//拍摄点云
	pcl::PLYWriter writer;
	std::string PLYname = name;// +".ply";
	//writer.write<PointT>("myply.ply", *m_cloudModel, false, true);
	writer.write<pcl::PointXYZ>(PLYname.c_str(), *pCloudScene, false, true);
	return 0;
}

int AlgorithmPclPackage::saveCloudToPCD(const std::string& name, pcl::PointCloud<pcl::PointXYZ>::Ptr pCloudScene)//保存 点云.pcd
{
	pcl::PLYWriter writer;
	std::string PCDname = name;
	pcl::io::savePCDFile(PCDname.c_str(), *(pCloudScene));
	return 0;
}

//保存点云，支持PLY PCD 自动区分
int AlgorithmPclPackage::saveCloud(const std::string& name, pcl::PointCloud<pcl::PointXYZ>::Ptr pCloudScene)
{

	std::string suffix = name.substr(name.size() - 3, 3);
	int iRes = 0;
	if (suffix == "ply") {
		iRes = saveCloudToPLY(name, pCloudScene);
	} else if (suffix == "pcd") {
		iRes = saveCloudToPCD(name, pCloudScene);
	}

	return iRes;
}


int AlgorithmPclPackage::saveCloudToPLY(const std::string& name, pcl::PointCloud<pcl::PointXYZLNormal>::Ptr pCloudScene)//保存 点云.ply
{
	//pcl::PointCloud<pcl::PointXYZ>::Ptr pCloudScene;
	//pCloudScene.reset(new pcl::PointCloud<pcl::PointXYZ>);//点云
	//pCloudScene = m_pCloud;//拍摄点云
	pcl::PLYWriter writer;
	std::string PLYname = name;// +".ply";
	//writer.write<PointT>("myply.ply", *m_cloudModel, false, true);
	writer.write<pcl::PointXYZLNormal>(PLYname.c_str(), *pCloudScene, false, true);
	return 0;
}

int AlgorithmPclPackage::saveCloudToPCD(const std::string& name, pcl::PointCloud<pcl::PointXYZLNormal>::Ptr pCloudScene)//保存 点云.pcd
{
	pcl::PLYWriter writer;
	std::string PCDname = name;
	pcl::io::savePCDFile(PCDname.c_str(), *(pCloudScene));
	return 0;
}

//保存点云，支持PLY PCD 自动区分
int AlgorithmPclPackage::saveCloud(const std::string& name, pcl::PointCloud<pcl::PointXYZLNormal>::Ptr pCloudScene)
{

	std::string suffix = name.substr(name.size() - 3, 3);
	int iRes = 0;
	if (suffix == "ply") {
		iRes = saveCloudToPLY(name, pCloudScene);
	} else if (suffix == "pcd") {
		iRes = saveCloudToPCD(name, pCloudScene);
	}

	return iRes;
}



//单位转换
void AlgorithmPclPackage::CloudUnitRatio(pcl::PointCloud<pcl::PointXYZ>::Ptr& pCloud, float fRatio)
{
	for (int i = 0; i < pCloud->points.size(); i++) {
		auto& p = pCloud->points[i];
		p.x *= fRatio;
		p.y *= fRatio;
		p.z *= fRatio;
	}
}

//点云转法向量点云
pcl::PointCloud<pcl::Normal>::Ptr AlgorithmPclPackage::PCLCloudXYZ2Normal(pcl::PointCloud<pcl::PointXYZ>::Ptr& pCloud, int iK_nearest)
{
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
	tree->setInputCloud(pCloud);
	n.setInputCloud(pCloud);
	n.setSearchMethod(tree);
	n.setKSearch(iK_nearest);
	n.compute(*normals);
	return normals;
}

//点云转法向量点云
pcl::PointCloud<pcl::PointXYZLNormal>::Ptr AlgorithmPclPackage::PCLCloudXYZ2XYZNormal(pcl::PointCloud<pcl::PointXYZ>::Ptr& pCloud, int iK_nearest)
{

	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
	normals = PCLCloudXYZ2Normal(pCloud, iK_nearest);

	pcl::PointCloud<pcl::PointXYZLNormal>::Ptr Cloud_xyznormals(new pcl::PointCloud<pcl::PointXYZLNormal>);
	pcl::PointXYZLNormal xyz;
	for (int i = 0; i < pCloud->size(); i++) {
		xyz.x = pCloud->points[i].x;
		xyz.y = pCloud->points[i].y;
		xyz.z = pCloud->points[i].z;
		xyz.normal_x = normals->points[i].normal_x;
		xyz.normal_y = normals->points[i].normal_y;
		xyz.normal_z = normals->points[i].normal_z;
		xyz.curvature = normals->points[i].curvature;
		Cloud_xyznormals->points.push_back(xyz);
	}
	Cloud_xyznormals->width = pCloud->size();
	Cloud_xyznormals->height = 1;

	return Cloud_xyznormals;
}


//调试用
//显示点云
void AlgorithmPclPackage::ShowCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr& pcloud, const std::string& sViewerName)
{
	pcl::visualization::CloudViewer viewer(sViewerName);//窗口
//showCloud函数是同步的，在此处等待直到渲染显示为止
	viewer.showCloud(pcloud);//显示点云
	////https://blog.csdn.net/weixin_42585456/article/details/121176411
	//viewer.setCameraPosition(0, 0, -1, 0, 0, 1, 0, -1, 0);
	////该注册函数在可视化时只调用一次
	//viewer.runOnVisualizationThreadOnce(viewerOneOff);//初始化显示线程函数
	////该注册函数在渲染输出时每次都调用
	//viewer.runOnVisualizationThread(viewerPsycho);//显示回调函数
	while (!viewer.wasStopped())//等待窗口结束
	{
		//在此处可以添加其他处理
		//user_data++;
	}

}
//开一个线程显示点云
void AlgorithmPclPackage::ShowCloudUseThread(const pcl::PointCloud<pcl::PointXYZ>::Ptr& pcloud, const std::string& sViewerName)
{
	//判断是否是新线程，是的话新建建线程并显示点云，不是的话已知线程、窗口中更新点云


	bool bReadedSignal = false;//子线程读取数据完成的信号
	std::thread saveALLImageThread([&]
		{
			std::string sName = sViewerName;//bug:形参无法进入线程

			//bReadedSignal = true;//将容易析构的变量放在以上部分
			pcl::PointCloud<pcl::PointXYZ>::Ptr pCloudScene;
			pCloudScene.reset(new pcl::PointCloud<pcl::PointXYZ>);//点云
			pcl::copyPointCloud<pcl::PointXYZ, pcl::PointXYZ>(*pcloud, *pCloudScene);

			bReadedSignal = true;//将容易析构的变量放在以上部分


			//判断是否是新窗口，是的h话新建并显示点云，不是的话已知窗口中更新点云


			pcl::visualization::CloudViewer viewer(sName);//窗口
			//showCloud函数是同步的，在此处等待直到渲染显示为止
			viewer.showCloud(pCloudScene);//显示点云

			//该注册函数在可视化时只调用一次
			//viewer.runOnVisualizationThreadOnce(viewerOneOff);//初始化显示线程函数
			////该注册函数在渲染输出时每次都调用
			//viewer.runOnVisualizationThread(viewerPsycho);//显示回调函数
			while (!viewer.wasStopped())//等待窗口结束
			{
				//在此处可以添加其他处理
				//user_data++;
				Sleep(10);
			}

			//pcl::PointCloud<pcl::PointXYZ>::Ptr pCloudScene;
			////pCloudScene.reset(new pcl::PointCloud<pcl::PointXYZ>);//点云
			//pcl::copyPointCloud<pcl::PointXYZ, pcl::PointXYZ>(*m_pCloud, *pCloudScene);
			//pCloudScene = m_pCloud;//拍摄点云

			//pcl::PLYWriter writer;
			//std::string PLYname = name3 + ".ply";
			////writer.write<PointT>("myply.ply", *m_cloudModel, false, true);
			//writer.write<pcl::PointXYZ>(PLYname.c_str(), *pCloudScene, false, true);
		});
	saveALLImageThread.detach();//线程分离
	//Sleep(5);//等待线程启动后读取参数，防止主线程析构参数
	int iWhileTime = 0;
	while (!bReadedSignal && iWhileTime < 1000) {
		iWhileTime++;
		Sleep(1);
	}
	return;

}


//---------------------------------利用独立线程VTK显示点云------------------------------------//
//在一个线程中显示点云并分离线程
void AlgorithmPclPackage::ShowCloudWithThreadDetach(PointCloudT::Ptr& pCloud, std::string sCloudName)
{
	// 声明一个线程处理任务  传入匿名函数  在声明后直接开启一个线程执行匿名函数体
	std::thread ShowCloudThread([&]
		{
			// runResult = 连接网络 、拷贝文件、等等耗时操作  实际执行的任务放在这个位置执行
			std::cout << "显示点云" + sCloudName + "线程开始" << std::endl;
			//显示绘制
			PointCloudT::Ptr m_cloudObstacleThread;
			m_cloudObstacleThread.reset(new pcl::PointCloud<pcl::PointXYZ>);
			pcl::copyPointCloud<PointT, PointT>(*pCloud, *m_cloudObstacleThread);
			boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer(sCloudName));
			viewer->setBackgroundColor(1, 1, 1);
			viewer->addCoordinateSystem(1.0);
			viewer->initCameraParameters();
			viewer->addPointCloud<pcl::PointXYZ>(m_cloudObstacleThread, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(m_cloudObstacleThread, 0, 255, 0), sCloudName);
			viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, sCloudName);
			while (!viewer->wasStopped()) {
				viewer->spinOnce(100);
				//boost::this_thread::sleep(boost::posix_time::microseconds(100000));
				Sleep(10);
			}
			//Sleep(5000);
			std::cout << "显示点云" + sCloudName + "线程结束" << std::endl;
			//runResult = true;
			// 执行完成后 发出信号  告知线程执行结束
			//emit signalRunOver();
		});
	// 分离线程  让其自生自灭(销毁线程)
	ShowCloudThread.detach();
	//等待线程执行结束
	//ShowCloudThread.join();
}
//在一个线程中显示点云并等待线程结束
void ShowCloudWithThreadJoin(PointCloudT::Ptr& pCloud, std::string sCloudName)
{
	// 声明一个线程处理任务  传入匿名函数  在声明后直接开启一个线程执行匿名函数体
	std::thread ShowCloudThread([&]
		{
			// runResult = 连接网络 、拷贝文件、等等耗时操作  实际执行的任务放在这个位置执行
			std::cout << "显示点云" + sCloudName + "线程开始" << std::endl;
			//显示绘制
			PointCloudT::Ptr m_cloudObstacleThread;
			m_cloudObstacleThread.reset(new pcl::PointCloud<pcl::PointXYZ>);
			pcl::copyPointCloud<PointT, PointT>(*pCloud, *m_cloudObstacleThread);
			boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer(sCloudName));
			viewer->setBackgroundColor(1, 1, 1);
			viewer->addCoordinateSystem(1.0);
			viewer->initCameraParameters();
			viewer->addPointCloud<pcl::PointXYZ>(m_cloudObstacleThread, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(m_cloudObstacleThread, 0, 255, 0), sCloudName);
			viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, sCloudName);
			while (!viewer->wasStopped()) {
				viewer->spinOnce(100);
				//boost::this_thread::sleep(boost::posix_time::microseconds(100000));
			}
			//Sleep(5000);
			std::cout << "显示点云" + sCloudName + "线程结束" << std::endl;
			//runResult = true;
			// 执行完成后 发出信号  告知线程执行结束
			//emit signalRunOver();
		});
	// 分离线程  让其自生自灭(销毁线程)
	//ShowCloudThread.detach();
	//等待线程执行结束
	ShowCloudThread.join();
}


//剪切一定范围的点云
void AlgorithmPclPackage::CropCloudRange(PointCloudT::Ptr& pCloud, float fMinDistX, float fMaxDistX, float fMinDistY, float fMaxDistY, float fMinDistZ, float fMaxDistZ, PointCloudT::Ptr& pCropCloud)
{
	pCropCloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
	if (pCloud->size() <= 0)
		return;

	// 创建条件对象
	pcl::ConditionAnd<PointT>::Ptr range_cond(new pcl::ConditionAnd<PointT>());
	range_cond->addComparison(pcl::FieldComparison<PointT>::ConstPtr(new
		pcl::FieldComparison<PointT>("x", pcl::ComparisonOps::GT, fMinDistX)));//增加比较算子:y>0
	range_cond->addComparison(pcl::FieldComparison<PointT>::ConstPtr(new
		pcl::FieldComparison<PointT>("x", pcl::ComparisonOps::LT, fMaxDistX)));//增加比较算子:y<0.8
	range_cond->addComparison(pcl::FieldComparison<PointT>::ConstPtr(new
		pcl::FieldComparison<PointT>("y", pcl::ComparisonOps::GT, fMinDistY)));//增加比较算子:y>0
	range_cond->addComparison(pcl::FieldComparison<PointT>::ConstPtr(new
		pcl::FieldComparison<PointT>("y", pcl::ComparisonOps::LT, fMaxDistY)));//增加比较算子:y<0.8
	range_cond->addComparison(pcl::FieldComparison<PointT>::ConstPtr(new
		pcl::FieldComparison<PointT>("z", pcl::ComparisonOps::GT, fMinDistZ)));//增加比较算子:y>0
	range_cond->addComparison(pcl::FieldComparison<PointT>::ConstPtr(new
		pcl::FieldComparison<PointT>("z", pcl::ComparisonOps::LT, fMaxDistZ)));//增加比较算子:y<0.8
	// 创建滤波器
	pcl::ConditionalRemoval<PointT> condrem;//条件剔除滤波器
	condrem.setCondition(range_cond);
	condrem.setInputCloud(pCloud);
	condrem.setKeepOrganized(false);//不保持点云结构
	condrem.filter(*pCropCloud);//滤波
}
void AlgorithmPclPackage::CropCloudRange(PointCloudT::Ptr& pCloud, TCloudRange tCloudRange, PointCloudT::Ptr& pCropCloud)
{
	CropCloudRange(pCloud, tCloudRange.fMinDistX, tCloudRange.fMaxDistX, tCloudRange.fMinDistY, tCloudRange.fMaxDistY, tCloudRange.fMinDistZ, tCloudRange.fMaxDistZ, pCropCloud);
}

//手动判断范围剪切点云
void AlgorithmPclPackage::CropCloudRange2(PointCloudT::Ptr& pCloud, float fMinDistX, float fMaxDistX, float fMinDistY, float fMaxDistY, float fMinDistZ, float fMaxDistZ, PointCloudT::Ptr& pCropCloud)
{
	pCropCloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
	//pcl::PointCloud<pcl::PointXYZ> o3dPointCloud_result;
	if (pCloud->size() <= 0)
		return;

	//pcl::ConditionAnd<pcl::PointXYZ>::Ptr range_cloud(new pcl::ConditionAnd<pcl::PointXYZ>());
	////设置作用域为x，取大于xmin且小于xmax的位置，保留在点云中，其余进行移除
	//if (xmin < xmax)
	//{
	//	range_cloud->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZ>("x", pcl::ComparisonOps::GT, xmin)));
	//	range_cloud->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZ>("x", pcl::ComparisonOps::LT, xmax)));
	//}
	//if (ymin < ymax)
	//{
	//	range_cloud->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZ>("y", pcl::ComparisonOps::GT, ymin)));
	//	range_cloud->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZ>("y", pcl::ComparisonOps::LT, ymax)));
	//}
	//if (zmin < zmax)
	//{
	//	range_cloud->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::GT, zmin)));
	//	range_cloud->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::LT, zmax)));
	//}
	//pcl::ConditionalRemoval<pcl::PointXYZ> condream;
	//condream.setCondition(range_cloud);
	//condream.setInputCloud(o3dPointCloud.makeShared());
	//condream.setKeepOrganized(true);
	//condream.filter(o3dPointCloud_result);

	//std::vector<int> mapping_cut;
	//pcl::removeNaNFromPointCloud(o3dPointCloud_result, o3dPointCloud_result, mapping_cut);
	//swx::手写的循环比调用PCL函数快。
	for (int i = 0; i < pCloud->size(); i++) {
		if (pCloud->points[i].x<fMinDistX || pCloud->points[i].x >fMaxDistX || pCloud->points[i].y<fMinDistY || pCloud->points[i].y >fMaxDistY || pCloud->points[i].z<fMinDistZ || pCloud->points[i].z >fMaxDistZ)continue;
		pCropCloud->push_back(pCloud->points[i]);
	}
	return;
}

void AlgorithmPclPackage::CropCloudRange2(PointCloudT::Ptr& pCloud, TCloudRange tCloudRange, PointCloudT::Ptr& pCropCloud)
{
	CropCloudRange2(pCloud, tCloudRange.fMinDistX, tCloudRange.fMaxDistX, tCloudRange.fMinDistY, tCloudRange.fMaxDistY, tCloudRange.fMinDistZ, tCloudRange.fMaxDistZ, pCropCloud);
}

//去除无效点云
void AlgorithmPclPackage::removeNaNCloud(PointCloudT::Ptr& pNaNCloud, PointCloudT::Ptr& pCloud)
{
	//pCloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
	std::vector<int> index;
	pcl::removeNaNFromPointCloud(*pNaNCloud, *pCloud, index);
}

//采样滤波
void AlgorithmPclPackage::SamplingFilter(PointCloudT::Ptr& pCloud, float fLeafX, float fLeafY, float fLeafZ, PointCloudT::Ptr& pSamplingCloud)
{
	//pSamplingCloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::VoxelGrid<PointT> m_grid;// 下采样
	m_grid.setLeafSize(fLeafX, fLeafY, fLeafZ);
	m_grid.setInputCloud(pCloud);
	m_grid.filter(*pSamplingCloud);
}


//半径滤波
void AlgorithmPclPackage::RadiusFilter(PointCloudT::Ptr& pCloud, float fRadius, float fMinNeighbors, PointCloudT::Ptr& pRadiusFilterCloud)
{
	//pRadiusFilterCloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
	//半径异常剔除滤波器
	pcl::RadiusOutlierRemoval<PointT> outrem;//半径异常剔除 滤波器
	outrem.setRadiusSearch(fRadius);
	outrem.setMinNeighborsInRadius(fMinNeighbors);
	//outrem.setRadiusSearch(0.02);//0.03半径范围内应至少有5个点
	//outrem.setMinNeighborsInRadius(5);
	outrem.setInputCloud(pCloud);
	outrem.filter(*pRadiusFilterCloud);//滤波
}

//统计滤波  (比较耗时)
void AlgorithmPclPackage::StatisticalFilter(PointCloudT::Ptr& pCloud, int iMeanK, float fStddevThresh, PointCloudT::Ptr& pStatisticalFilterCloud)
{
	//pStatisticalFilterCloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::StatisticalOutlierRemoval<PointT> sor; //创建滤波器对象
	sor.setMeanK(iMeanK);//K邻域临近点个数50
	sor.setStddevMulThresh(fStddevThresh);//标准差倍数1
	sor.setInputCloud(pCloud);
	sor.filter(*pStatisticalFilterCloud);//滤波
}


//添加高斯噪声
void AlgorithmPclPackage::AddGaussNoise(PointCloudT::Ptr& pCloud, float fMu, float fSigma, PointCloudT::Ptr& pGaussNoiseCloud)
{
	//-------------------添加高斯噪声-------------------
	if (fSigma > 0) {
		boost::mt19937 zgy;                             //等分布均匀伪随机数发生器
		zgy.seed(static_cast<unsigned int>(time(0))); //随机种子
		boost::normal_distribution<> nd(fMu, fSigma);   //定义正态分布，均值为mu，标准差为sigma
		boost::variate_generator<boost::mt19937&, boost::normal_distribution<>> gauss_noise(zgy, nd);   //生成高斯噪声

		pcl::PointCloud<pcl::PointXYZ> cloud_gauss;   //声明高斯噪声点云
		cloud_gauss = *pCloud;                   //将原始点云拷贝给高斯噪声点云，用于下面的平移                

		for (size_t i = 0; i < cloud_gauss.size(); i++) {
			cloud_gauss.points[i].x += static_cast<float> (gauss_noise());
			cloud_gauss.points[i].y += static_cast<float> (gauss_noise());
			cloud_gauss.points[i].z += static_cast<float> (gauss_noise());
		}

		//*pGaussNoiseCloud = *pCloud + cloud_gauss;   //将原始点云与噪声点云合并，得到添加高斯噪声后的点云
		*pGaussNoiseCloud = cloud_gauss;
	} else {
		pGaussNoiseCloud = pCloud;
	}
}

//剪切特定方向点云 //根据向量方向进行裁剪
void AlgorithmPclPackage::CropCloudNormalRange(PointCloudT::Ptr& pCloud, float fNormalAngleRange, PointCloudT::Ptr& pNormalRangeCloud, char Axis)
{
	pcl::PointCloud<pcl::PointXYZ> o3dPointCloud_result;

	if (pCloud->size() == 0)return;

	pcl::PointXYZ xyz;
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
	tree->setInputCloud(pCloud);
	n.setInputCloud(pCloud);
	n.setSearchMethod(tree);
	n.setKSearch(10);
	n.compute(*normals);//swx:计算法向量。

	float fNormalAngleRange_rad = fNormalAngleRange * PI / 180;

	if (Axis == 'z' || Axis == 'Z') {
		for (int i = 0; i < pCloud->size(); i++) {

			float normal_x = normals->points[i].normal_x;
			float normal_y = normals->points[i].normal_y;
			float normal_z = normals->points[i].normal_z;
			float curvature = normals->points[i].curvature;//曲率

			//float z_normal_angle = abs(std::atan((normal_y / sqrt(normal_x * normal_x + normal_z * normal_z))));//swx:平面与向量的角度
			float z_normal_angle = abs(std::atan((sqrt(normal_x * normal_x + normal_y * normal_y) / normal_z)));//swx:平面与向量的角度

			if (z_normal_angle < fNormalAngleRange_rad) {
				xyz.x = pCloud->points[i].x;
				xyz.y = pCloud->points[i].y;
				xyz.z = pCloud->points[i].z;
				o3dPointCloud_result.points.push_back(xyz);
			}
		}
	} else if (Axis == 'y' || Axis == 'Y') {
		for (int i = 0; i < pCloud->size(); i++) {

			float normal_x = normals->points[i].normal_x;
			float normal_y = normals->points[i].normal_y;
			float normal_z = normals->points[i].normal_z;
			float curvature = normals->points[i].curvature;//曲率


			//float z_normal_angle = abs(std::atan((normal_y / sqrt(normal_x * normal_x + normal_z * normal_z))));//swx:平面与向量的角度
			float z_normal_angle = abs(std::atan((sqrt(normal_x * normal_x + normal_z * normal_z) / normal_y)));//swx:平面与向量的角度

			if (z_normal_angle < fNormalAngleRange_rad) {
				xyz.x = pCloud->points[i].x;
				xyz.y = pCloud->points[i].y;
				xyz.z = pCloud->points[i].z;
				o3dPointCloud_result.points.push_back(xyz);
			}
		}
	} else if (Axis == 'x' || Axis == 'X') {
		for (int i = 0; i < pCloud->size(); i++) {

			float normal_x = normals->points[i].normal_x;
			float normal_y = normals->points[i].normal_y;
			float normal_z = normals->points[i].normal_z;
			float curvature = normals->points[i].curvature;//曲率


			//float z_normal_angle = abs(std::atan((normal_y / sqrt(normal_x * normal_x + normal_z * normal_z))));//swx:平面与向量的角度
			float z_normal_angle = abs(std::atan((sqrt(normal_y * normal_y + normal_z * normal_z) / normal_x)));//swx:平面与向量的角度

			if (z_normal_angle < fNormalAngleRange_rad) {
				xyz.x = pCloud->points[i].x;
				xyz.y = pCloud->points[i].y;
				xyz.z = pCloud->points[i].z;
				o3dPointCloud_result.points.push_back(xyz);
			}
		}
	}


	pNormalRangeCloud = o3dPointCloud_result.makeShared();//注意可能的bug 会不会析构

}

//---------------------------------聚类------------------------------------//
//欧式聚类提取 （欧几里得聚类提取）
void AlgorithmPclPackage::EuclideanClusterExtractionCloud(const PointCloudT::Ptr& pCloud, float fClusterTolerance, int iMinClusterSize, int iMaxClusterSize, std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>* pvCloudPtr)
{
	// Creating the KdTree object for the search method of the extraction为提取的搜索方法创建KdTree对象
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
	tree->setInputCloud(pCloud);

	std::vector<pcl::PointIndices> cluster_indices;//提取的聚类
	pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;//欧式聚类提取 （欧几里得聚类提取）
	//ec.setClusterTolerance(0.02); //簇的容差 2cm
	//ec.setMinClusterSize(50);//最小簇
	//ec.setMaxClusterSize(9999999999);//最大簇
	ec.setClusterTolerance(fClusterTolerance); //簇的容差 2cm
	ec.setMinClusterSize(iMinClusterSize);//最小簇
	ec.setMaxClusterSize(iMaxClusterSize);//最大簇
	ec.setSearchMethod(tree);//kd树查找模式
	ec.setInputCloud(pCloud);
	ec.extract(cluster_indices);//提取

	//遍历每一个聚类
	int j = 0;
	std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> vCloudPtr;
	for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
		for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++)
			cloud_cluster->points.push_back(pCloud->points[*pit]); //*
		cloud_cluster->width = cloud_cluster->points.size();
		cloud_cluster->height = 1;
		cloud_cluster->is_dense = true;

		vCloudPtr.push_back(cloud_cluster);
		//std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size() << " data points." << std::endl;
		//std::stringstream ss;
		//ss << "cloud_cluster_" << j << ".pcd";
		//writer.write<pcl::PointXYZ>(ss.str(), *cloud_cluster, false); //*
		j++;
	}
	//return vCloudPtr;
	*pvCloudPtr = vCloudPtr;

}

//获取最大点云 点数最多的点云
void AlgorithmPclPackage::getMaxCloud(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> vCloudPtr, pcl::PointCloud < pcl::PointXYZ>::Ptr* pCloudPtr)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr Cloud2 = getMaxCloud(vCloudPtr);
	*pCloudPtr = Cloud2;
}
pcl::PointCloud < pcl::PointXYZ>::Ptr AlgorithmPclPackage::getMaxCloud(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> vCloudPtr)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr Cloud2;
	if (vCloudPtr.size() == 0) {
		return Cloud2;
	}
	//最大点云
	int iMaxCloudIndex = 0;
	int iMaxSize = 0;
	for (int i = 0; i < vCloudPtr.size(); i++) {
		int isize = vCloudPtr[i]->points.size();
		if (isize > iMaxSize) {
			iMaxSize = isize;
			iMaxCloudIndex = i;
		}
	}
	Cloud2 = vCloudPtr[iMaxCloudIndex];
	return Cloud2;
}


//---------------------------------投影------------------------------------//
//变换点云
void AlgorithmPclPackage::TransformPointCloud(PointCloudT::Ptr& pCloud, Eigen::Matrix4f M4fTransformMatrix, PointCloudT::Ptr& pTranCloud)
{
	pcl::transformPointCloud(*pCloud, *pTranCloud, M4fTransformMatrix);
}

//投影点云到一个平面
void AlgorithmPclPackage::ProjectOntoPlane(PointCloudT::Ptr& pCloud, float fa, float fb, float fc, float fd, PointCloudT::Ptr& pProjectCloud)
{
	//可选线、平面、圆、球等模型
	// 创建一个系数为X=Y=0,Z=1的平面 ax+by+cz+d=0
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
	coefficients->values.resize(4);//a b c d 系数
	coefficients->values[0] = fa;
	coefficients->values[1] = fb;
	coefficients->values[2] = fc;
	coefficients->values[3] = fd;
	// 创建滤波器对象
	pcl::ProjectInliers<PointT> proj;//投影滤波对象
	proj.setModelType(pcl::SACMODEL_PLANE);//平面模式
	proj.setInputCloud(pCloud);//输入点云
	proj.setModelCoefficients(coefficients);//设置模型系数
	proj.filter(*pProjectCloud);//滤波
}

//
//std::vector<cv::Point3f> CloudPoint2vP3f(const pcl::PointCloud<pcl::PointXYZ>::Ptr& Cloud)
//{
//	int size = Cloud->points.size();
//	std::vector<cv::Point3f>  vp3fCloud(size);
//	for (int i = 0; i < Cloud->size(); i++)
//	{
//		cv::Point3f& point1 = vp3fCloud[i];
//		auto & point2 = Cloud->points[i];
//		point1.x = point2.x;
//		point1.y = point2.y;
//		point1.z = point2.z;
//	}
//	return vp3fCloud;
//}

////投影点云转深度图
//void ProjectionCloud2DepthImage(const pcl::PointCloud<pcl::PointXYZ>::Ptr& Cloud, const int& iImageRows, const int& iImageClos, const cv::Mat& matCameraMatrix, const cv::Mat& matDistCoeffs, cv::Mat* pdepthImage, bool bswapXY)
//{
//	//cvCloud2vPoint3f 有序点云转无序点云 0 0 0 点云为无效点云 仅转换一个 先set后转vector
//	std::vector<cv::Point3f> vP3f = CloudPoint2vP3f(Cloud);
//
//	std::vector<cv::Point2f> ObjectPoints2D;
//	Mat Rvec = Mat::eye(3, 3, CV_32FC1);
//	Mat Tvec = Mat::zeros(1, 3, CV_32FC1);
//	RMToRV(Rvec, Rvec);
//	//HomMat3dToRVT(mat3DObjectInCam, &Rvec, &Tvec);
//	cv::projectPoints(vP3f, Rvec, Tvec, matCameraMatrix, matDistCoeffs, ObjectPoints2D);
//
//	Mat depthImage = Mat::zeros(iImageRows, iImageClos, CV_16U);
//	//std::vector< std::vector<double>> vvdPoint = vPoint2vvd(ObjectPoints2D);
//	for (int i = 0; i < ObjectPoints2D.size(); i++)
//	{
//		int Px = std::round(ObjectPoints2D[i].x);
//		int Py = std::round(ObjectPoints2D[i].y);
//		if (bswapXY)
//		{
//			std::swap(Px, Py);
//		}
//		if (0 <= Px && Px < iImageRows && 0 <= Py && Py < iImageClos)
//		{
//			depthImage.at<ushort>(Px, Py) = vP3f[i].z;
//		}
//		else
//		{
//			//点云在相机视觉下看不到
//		}
//
//	}
//	*pdepthImage = depthImage;
//	//return depthImage;
//}

//PCL点云转Opencv图像
void AlgorithmPclPackage::CloudPoint2Mat(PointCloudT::Ptr& pCloud, float fResolution, float fRangeX, float fRangeY, float fOffsetX, float fOffsetY, cv::Mat* pPointCloudImage, int iStatisticalMode)
{
	//输入检验
	if (pPointCloudImage == NULL || pCloud->points.size() == 0) {
		return;
	}

	float fResolution2 = 1 / fResolution;//分辨率的倒数，加速计算  每m多少像素

	//平移场景点云，方便投影到图像
	PointCloudT::Ptr CloudOffset;
	CloudOffset.reset(new PointCloudT);//临时点云;
	Eigen::Matrix4d M4dTransformationMatrix = algorithmVision3D.EulerPose2MatrixPose(fOffsetX, fOffsetY, 0, 0, 0, 0);
	pcl::transformPointCloud(*pCloud, *CloudOffset, M4dTransformationMatrix);

	int iImageW = fRangeX / fResolution;
	int iImageH = fRangeY / fResolution;
	cv::Mat PointCloudImage(iImageW, iImageH, CV_8UC1);
	PointCloudImage = cv::Mat::zeros(PointCloudImage.size(), PointCloudImage.type());//图像置零

	if (iStatisticalMode == 1) {
		//遍历每一个点云计数到图像中
		for (int i = 0; i < CloudOffset->points.size(); i++) {
			//计算该点云在图像中的索引
			int iX = round(CloudOffset->points[i].x * fResolution2);
			int iY = round(CloudOffset->points[i].y * fResolution2);
			//if (iX > 0 && iX < iImageW && iY>0 && iY < iImageH)
			//{
			//	if (PointCloudImage.at<uchar>(iY, iX) < 255)
			//	{
			//		PointCloudImage.at<uchar>(iY, iX)++;
			//	}
			//}
			if (iX > 0 && iX < iImageH && iY>0 && iY < iImageW) {
				if (PointCloudImage.at<uchar>(iY, iX) < 255) {
					PointCloudImage.at<uchar>(iY, iX)++;
				}
			}
		}
	} else if (iStatisticalMode == 0) {
		//计算图像像素中心对应空间点周围是否有点云
		//kd树
		//float fRadius = 0.003;//查找半径
		float fRadius = fResolution;//查找半径设置为分辨率（30ms左右）
		// 在半径r内搜索近邻
		std::vector<int> pointIdxRadiusSearch;
		std::vector<float> pointRadiusSquaredDistance;
		//最近的的一个点
		std::vector<int>pointIdxNKNSearch(1);
		std::vector<float>pointNKNSquaredDistance(1);

		pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
		kdtree.setInputCloud(CloudOffset);//kd树输入点云

		for (int i = 0; i < iImageW; i++) {
			for (int j = 0; j < iImageH; j++) {
				//对应空间位置
				float fX = j * fResolution;
				float fY = i * fResolution;
				PointT SearchPoint;//空间位置点
				SearchPoint.x = fX;
				SearchPoint.y = fY;
				SearchPoint.z = 0;
				if (1) {
					//统计一定距离内的点数
					if (kdtree.radiusSearch(SearchPoint, fRadius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0) {
						PointCloudImage.at<uchar>(i, j) = pointIdxRadiusSearch.size();
					}
				} else {
					//距离不正常？？
					//一定距离内有点云就置1
					if (kdtree.nearestKSearch(SearchPoint, 1, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) {
						if (pointNKNSquaredDistance[0] < fRadius) {
							PointCloudImage.at<uchar>(i, j) = 1;
						}
					}
				}
			}
		}

	}

	//输出
	*pPointCloudImage = PointCloudImage;
}

//PCL点云转Opencv图像
void AlgorithmPclPackage::CloudXYPlane2Mat(PointCloudT::Ptr& pCloud, float fResolution, float fRangeX, float fRangeY, float fOffsetX, float fOffsetY, cv::Mat* pPointCloudImage, int iStatisticalMode)
{
	//输入检验
	if (pPointCloudImage == NULL || pCloud->points.size() == 0) {
		return;
	}

	float fResolution2 = 1 / fResolution;//分辨率的倒数，加速计算  每m多少像素

	//平移场景点云，方便投影到图像
	PointCloudT::Ptr CloudOffset;
	CloudOffset.reset(new PointCloudT);//临时点云;
	Eigen::Matrix4d M4dTransformationMatrix = algorithmVision3D.EulerPose2MatrixPose(fOffsetX, fOffsetY, 0, 0, 0, 0);
	pcl::transformPointCloud(*pCloud, *CloudOffset, M4dTransformationMatrix);

	int iImageW = fRangeX / fResolution;
	int iImageH = fRangeY / fResolution;
	cv::Mat PointCloudImage(iImageW, iImageH, CV_8UC1);
	PointCloudImage = cv::Mat::zeros(PointCloudImage.size(), PointCloudImage.type());//图像置零

	if (iStatisticalMode == 1) {
		//遍历每一个点云计数到图像中
		for (int i = 0; i < CloudOffset->points.size(); i++) {
			//计算该点云在图像中的索引
			int iX = round(CloudOffset->points[i].x * fResolution2);
			int iY = round(CloudOffset->points[i].y * fResolution2);
			//if (iX > 0 && iX < iImageW && iY>0 && iY < iImageH)
			//{
			//	if (PointCloudImage.at<uchar>(iY, iX) < 255)
			//	{
			//		PointCloudImage.at<uchar>(iY, iX)++;
			//	}
			//}
			if (iX > 0 && iX < iImageH && iY>0 && iY < iImageW) {
				if (PointCloudImage.at<uchar>(iY, iX) < 255) {
					PointCloudImage.at<uchar>(iY, iX)++;
				}
			}
		}
	} else if (iStatisticalMode == 0) {
		//计算图像像素中心对应空间点周围是否有点云
		//kd树
		//float fRadius = 0.003;//查找半径
		float fRadius = fResolution;//查找半径设置为分辨率（30ms左右）
		// 在半径r内搜索近邻
		std::vector<int> pointIdxRadiusSearch;
		std::vector<float> pointRadiusSquaredDistance;
		//最近的的一个点
		std::vector<int>pointIdxNKNSearch(1);
		std::vector<float>pointNKNSquaredDistance(1);

		pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
		kdtree.setInputCloud(CloudOffset);//kd树输入点云

		for (int i = 0; i < iImageW; i++) {
			for (int j = 0; j < iImageH; j++) {
				//对应空间位置
				float fX = j * fResolution;
				float fY = i * fResolution;
				PointT SearchPoint;//空间位置点
				SearchPoint.x = fX;
				SearchPoint.y = fY;
				SearchPoint.z = 0;
				if (1) {
					//统计一定距离内的点数
					if (kdtree.radiusSearch(SearchPoint, fRadius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0) {
						PointCloudImage.at<uchar>(i, j) = pointIdxRadiusSearch.size();
					}
				} else {
					//距离不正常？？
					//一定距离内有点云就置1
					if (kdtree.nearestKSearch(SearchPoint, 1, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) {
						if (pointNKNSquaredDistance[0] < fRadius) {
							PointCloudImage.at<uchar>(i, j) = 1;
						}
					}
				}
			}
		}

	}

	//输出
	*pPointCloudImage = PointCloudImage;
}

//PCL点云转Opencv点云
cv::Mat AlgorithmPclPackage::PCLCloud2cvCloud(PointCloudT::Ptr& pCloud, const int& iImageRows, const int& iImageClos, const cv::Mat& matCameraMatrix, const cv::Mat& matDistCoeffs, cv::Mat* DepthMask)
{
	//3D点云数据转化
	std::vector<cv::Point3f> CloudPoints;//空间点
	CloudPoints.reserve(pCloud->points.size());//预分配空间
	for (const auto& point : pCloud->points) {
		if (/*!std::isnan(point.x) && !std::isnan(point.y) && */!std::isnan(point.z)) {
			CloudPoints.push_back(cv::Point3f(point.x, point.y, point.z));
			//int x = static_cast<int>(point.x);
			//int y = static_cast<int>(point.y);
			//depth_image.at<float>(y, x) = point.z;
		}
	}

	//把点投影到2D平面上
	//相机位置
	Eigen::Matrix4d CamPose = algorithmVision3D.EulerPose2MatrixPose(0, 0, 0, 0, 0, 0);
	cv::Mat CamPoseMat = algorithmVision3D.Matrix4d2Mat4d(CamPose);
	Mat Rvec, Tvec;
	algorithmVision.HomMat3dToRVT(CamPoseMat, &Rvec, &Tvec);

	std::vector<cv::Point2f> ImagePoints;//绘制箭头投影图像点
	cv::projectPoints(CloudPoints, Rvec, Tvec, matCameraMatrix, matDistCoeffs, ImagePoints);

	//点云赋值

	cv::Mat cvCloud = cv::Mat::zeros(iImageRows, iImageClos, CV_32FC3);//有序点云
	cv::Mat cvCloud_mask;
	if (DepthMask != NULL) {
		cvCloud_mask = cv::Mat::zeros(iImageRows, iImageClos, CV_8UC1);//有序点云
	}
	for (int i = 0; i < ImagePoints.size(); i++) {
		const cv::Point2f& point = ImagePoints[i];
		int px = round(point.x);
		int py = round(point.y);

		if (px > 0 && px < iImageClos && py>0 && py < iImageRows) {
			const cv::Point3f& point3D = CloudPoints[i];
			Vec3f PixCloud = { point3D.x ,point3D.y ,point3D.z };
			cvCloud.at<Vec3f>(py, px) = PixCloud;

			if (DepthMask != NULL) cvCloud_mask.at<uchar>(py, px) = 255;
		}
	}


	if (DepthMask != NULL) {
		*DepthMask = cvCloud_mask;
	}

	return cvCloud;

	//投影点云转深度图
	//cv::Mat depth2;
	//ProjectionCloud2DepthImage(cvCloud, depth.rows, depth.cols, depthIntrinsic, depthDistortion, &depth2, true);
}

//投影点云转深度图
cv::Mat AlgorithmPclPackage::ProjectionCloud2DepthImage(const pcl::PointCloud<pcl::PointXYZ>::Ptr& pCloud, const int& iImageRows, const int& iImageClos, const cv::Mat& matCameraMatrix, const cv::Mat& matDistCoeffs, bool bswapXY)
{
	//3D点云数据转化
	std::vector<cv::Point3f> CloudPoints;//空间点
	CloudPoints.reserve(pCloud->points.size());//预分配空间
	for (const auto& point : pCloud->points) {
		if (/*!std::isnan(point.x) && !std::isnan(point.y) && */!std::isnan(point.z)) {
			CloudPoints.push_back(cv::Point3f(point.x, point.y, point.z));
			//int x = static_cast<int>(point.x);
			//int y = static_cast<int>(point.y);
			//depth_image.at<float>(y, x) = point.z;
		}
	}

	//把点投影到2D平面上
	//相机位置
	Eigen::Matrix4d CamPose = algorithmVision3D.EulerPose2MatrixPose(0, 0, 0, 0, 0, 0);
	cv::Mat CamPoseMat = algorithmVision3D.Matrix4d2Mat4d(CamPose);
	Mat Rvec, Tvec;
	algorithmVision.HomMat3dToRVT(CamPoseMat, &Rvec, &Tvec);

	std::vector<cv::Point2f> ImagePoints;//绘制箭头投影图像点
	cv::projectPoints(CloudPoints, Rvec, Tvec, matCameraMatrix, matDistCoeffs, ImagePoints);

	//点云赋值

	cv::Mat pdepthImage = cv::Mat::zeros(iImageRows, iImageClos, CV_32FC1);//有序点云

	for (int i = 0; i < ImagePoints.size(); i++) {
		const cv::Point2f& point = ImagePoints[i];
		int px = round(point.x);
		int py = round(point.y);

		if (px > 0 && px < iImageClos && py>0 && py < iImageRows) {
			//const cv::Point3f& point3D = CloudPoints[i];
			//Vec3f PixCloud = { point3D.x ,point3D.y ,point3D.z };
			//cvCloud.at<Vec3f>(py, px) = PixCloud;

			pdepthImage.at<float>(py, px) = CloudPoints.at(i).z * 1000;
		}
	}

	return pdepthImage;
}

//投影雷达转深度图
cv::Mat AlgorithmPclPackage::ProjectionLidar2DepthImage(const pcl::PointCloud<pcl::PointXYZ>::Ptr& pCloud1, const int& iImageRows, const int& iImageClos, const cv::Mat& matCameraMatrix, const cv::Mat& matLidarExtrinsic, const cv::Mat& matDistCoeffs, bool bswapXY)
{
	//转换到相机坐标系下
	Eigen::Matrix4f lidar2Cam = algorithmVision3D.Mat4d2Matrix4f(matLidarExtrinsic);
	PointCloudT::Ptr pCloud(new PointCloudT);
	pcl::transformPointCloud(*pCloud1, *pCloud, lidar2Cam);

	//3D点云数据转化
	std::vector<cv::Point3f> CloudPoints;//空间点
	CloudPoints.reserve(pCloud->points.size());//预分配空间
	for (const auto& point : pCloud->points) {
		if (!std::isnan(point.z)) {
			CloudPoints.push_back(cv::Point3f(point.x, point.y, point.z));
		}
	}

	//把点投影到2D平面上
	//相机位置
	Eigen::Matrix4d CamPose = algorithmVision3D.EulerPose2MatrixPose(0, 0, 0, 0, 0, 0);
	cv::Mat CamPoseMat = algorithmVision3D.Matrix4d2Mat4d(CamPose);
	Mat Rvec, Tvec;
	algorithmVision.HomMat3dToRVT(CamPoseMat, &Rvec, &Tvec);

	std::vector<cv::Point2f> ImagePoints;//绘制箭头投影图像点
	cv::projectPoints(CloudPoints, Rvec, Tvec, matCameraMatrix, matDistCoeffs, ImagePoints);

	//点云赋值

	cv::Mat pdepthImage = cv::Mat::zeros(iImageRows, iImageClos, CV_32FC1);//有序点云

	for (int i = 0; i < ImagePoints.size(); i++) {
		const cv::Point2f& point = ImagePoints[i];
		int px = round(point.x);
		int py = round(point.y);

		if (px > 0 && px < iImageClos && py>0 && py < iImageRows) {
			//const cv::Point3f& point3D = CloudPoints[i];
			//Vec3f PixCloud = { point3D.x ,point3D.y ,point3D.z };
			//cvCloud.at<Vec3f>(py, px) = PixCloud;

			pdepthImage.at<float>(py, px) = CloudPoints.at(i).z * 1000;
		}
	}

	return pdepthImage;
}

cv::Mat AlgorithmPclPackage::PCLCloud2cvCloudMask(PointCloudT::Ptr& pCloud, const int& iImageRows, const int& iImageClos, const cv::Mat& matCameraMatrix, const cv::Mat& matDistCoeffs)
{
	//3D点云数据转化
	std::vector<cv::Point3f> CloudPoints;//空间点
	CloudPoints.reserve(pCloud->points.size());//预分配空间
	for (const auto& point : pCloud->points) {
		if (/*!std::isnan(point.x) && !std::isnan(point.y) && */!std::isnan(point.z)) {
			CloudPoints.push_back(cv::Point3f(point.x, point.y, point.z));
			//int x = static_cast<int>(point.x);
			//int y = static_cast<int>(point.y);
			//depth_image.at<float>(y, x) = point.z;
		}
	}

	//把点投影到2D平面上
	//相机位置
	Eigen::Matrix4d CamPose = algorithmVision3D.EulerPose2MatrixPose(0, 0, 0, 0, 0, 0);
	cv::Mat CamPoseMat = algorithmVision3D.Matrix4d2Mat4d(CamPose);
	Mat Rvec, Tvec;
	algorithmVision.HomMat3dToRVT(CamPoseMat, &Rvec, &Tvec);

	std::vector<cv::Point2f> ImagePoints;//绘制箭头投影图像点
	cv::projectPoints(CloudPoints, Rvec, Tvec, matCameraMatrix, matDistCoeffs, ImagePoints);

	//点云赋值

	cv::Mat cvCloud_mask = cv::Mat::zeros(iImageRows, iImageClos, CV_8UC1);//有序点云

	for (int i = 0; i < ImagePoints.size(); i++) {
		const cv::Point2f& point = ImagePoints[i];
		int px = round(point.x);
		int py = round(point.y);

		if (px > 0 && px < iImageClos && py>0 && py < iImageRows) {
			//const cv::Point3f& point3D = CloudPoints[i];
			//Vec3f PixCloud = { point3D.x ,point3D.y ,point3D.z };
			//cvCloud.at<Vec3f>(py, px) = PixCloud;

			cvCloud_mask.at<uchar>(py, px) = 255;
		}
	}


	return cvCloud_mask;
}
//opencv点云转PCL点云
PointCloudT::Ptr AlgorithmPclPackage::cvCloud2PCLCloud(const cv::Mat& cvCloud, float fUnitConversionFactor)
{
	PointCloudT::Ptr  cloud(new pcl::PointCloud<pcl::PointXYZ>);
	//cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);

	//cloud->width = cvCloud.cols;
	//cloud->height = cvCloud.rows;
	cloud->is_dense = false;
	//cloud->points.resize(cloud->width * cloud->height);
	cloud->points.clear();
	//cloud->points[2].x
	//const float* pr_row = cvCloud.ptr<float>(0);
	for (int i = 0; i < cvCloud.rows; i++) {
		for (int j = 0; j < cvCloud.cols; j++) {
			const cv::Vec3f& CloudPoint = cvCloud.at<cv::Vec3f>(i, j);

			//float fX = CloudPoint.val[0];
			//float fY = CloudPoint.val[1];
			//float fZ = CloudPoint.val[2];
			if (abs(CloudPoint.val[0]) >= EPSILON || abs(CloudPoint.val[1]) >= EPSILON || abs(CloudPoint.val[2]) >= EPSILON)//不考虑浮点范围，可能有bug
			{
				pcl::PointXYZ pxyz;
				//pxyz.x = CloudPoint.val[0] * 0.001;
				//pxyz.y = CloudPoint.val[1] * 0.001;
				//pxyz.z = CloudPoint.val[2] * 0.001;
				pxyz.x = CloudPoint.val[0] * fUnitConversionFactor;
				pxyz.y = CloudPoint.val[1] * fUnitConversionFactor;
				pxyz.z = CloudPoint.val[2] * fUnitConversionFactor;
				cloud->points.push_back(pxyz);  //怎么解决效率问题？？
				//auto& p = cloud->points[i * cvCloud.cols + j];
				//p.x = fX;
				//p.y = fY;
				//p.z = fZ;
			} else {

			}

		}
	}
	cloud->width = cloud->points.size();
	cloud->height = 1;

	return cloud;

}
//---------------------------------计算特定值------------------------------------//
//计算某个x-y范围z的平均值
void AlgorithmPclPackage::CalculateMeanZ(PointCloudT::Ptr& pCloud, float fOffsetX, float fOffsetY, float fRangeX, float fRangeY, float fMinZ, float fMaxZ, float* pfMeanZ, int* piRangePointNum)
{

	if (pfMeanZ == NULL) {
		return;
	}

	PointCloudT::Ptr pCropCloud;//剪切点云
	pCropCloud.reset(new PointCloudT);

	CropCloudRange(pCloud, fOffsetX, fOffsetX + fRangeX, fOffsetY, fOffsetY + fRangeY, fMinZ, fMaxZ, pCropCloud);

	double dSumZ = 0;//z的距离和
	int iNum = pCropCloud->points.size();
	for (int i = 0; i < iNum; i++) {
		float fPointZ = pCropCloud->points[i].z;
		//cout << fPointZ << endl;
		dSumZ += fPointZ;
	}
	float fMeanZ = 0;
	if (pCropCloud->points.size() > 0) {
		fMeanZ = dSumZ / pCropCloud->points.size();//z的平均值
	}

	//输出
	*pfMeanZ = fMeanZ;
	if (piRangePointNum != NULL) {
		*piRangePointNum = pCropCloud->points.size();
	}
}
void AlgorithmPclPackage::CalculateMeanZ(PointCloudT::Ptr& pCloud, float fOffsetX, float fOffsetY, float fRangeX, float fRangeY, float* pfMeanZ, int* piRangePointNum)
{
	CalculateMeanZ(pCloud, fOffsetX, fOffsetY, fRangeX, fRangeY, FLT_MIN, FLT_MAX, pfMeanZ, piRangePointNum);
}

//计算点云的中值点 Z中值对应的点
pcl::PointXYZ AlgorithmPclPackage::CalculateCloudZMidValuePoint(PointCloudT::Ptr& pCloud)
{
	if (pCloud->points.size() == 0) {
		return pcl::PointXYZ(0, 0, 0);
	}
	//计算Z中值对应的点
	std::vector<float> vfZ(pCloud->points.size());
	for (int i = 0; i < pCloud->points.size(); i++) {
		vfZ[i] = pCloud->points[i].z;
	}
	std::vector<int> viIndex;
	//VectorSort(vfZ, 0, &viIndex);
	int iMidValueIndex = viIndex[pCloud->points.size() / 2];//中值对应的索引
	pcl::PointXYZ PointZMidValue = pCloud->points[iMidValueIndex];//中值对应的点
	return PointZMidValue;
}


//pCloudExtract 索引对应的点
void AlgorithmPclPackage::ExtractIndicesCloud(const PointCloudT::Ptr& pCloud, const pcl::PointIndices::Ptr& inliers, PointCloudT::Ptr* pCloudExtract)
{
	pCloudExtract->reset(new PointCloudT);
	pcl::ExtractIndices<pcl::PointXYZ> extract;//点云提取对象
			// 分离内层
	extract.setInputCloud(pCloud);//设置输入点云
	extract.setIndices(inliers);//设置分割后的内点点集
	extract.setNegative(false);//设置提取内点 而非外点
	extract.filter(**pCloudExtract);//提取

}



//点云拟合平面
void AlgorithmPclPackage::PlaneSegmentation(PointCloudT::Ptr& pCloud, float fMaxDist, pcl::ModelCoefficients::Ptr* pcoefficients, PointCloudT::Ptr* pCloudPlane)
{

	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);//模型系数对象 此处即平面系数
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices);//存内点索引集合对象
//创建分割对象
	pcl::SACSegmentation<pcl::PointXYZ> seg;
	seg.setOptimizeCoefficients(true);//可选：设置模型系数优化
	seg.setModelType(pcl::SACMODEL_PLANE);//必须:模型类型
	seg.setMethodType(pcl::SAC_RANSAC);//随机参数估计方法
	//float fMaxDist = 0.001;// 0.006;
	seg.setDistanceThreshold(fMaxDist);///距离阈值
	seg.setInputCloud(pCloud);//输入点云
	seg.segment(*inliers, *coefficients);//分割

	//if (inliers->indices.size() == 0)
	//{
	//	PCL_ERROR("Could not estimate a planar model for the given dataset.");
	//	//return (-1);
	//}
	//float fInteriorPointScale = 0;//内点比例
	//if (pCloud->size() != 0)
	//{
	//	fInteriorPointScale = inliers->indices.size() / pCloud->size();
	//}

	//std::cerr << "------------------------" << std::endl;
	//std::cerr << "Model coefficients: " << coefficients->values[0] << " "
	//	<< coefficients->values[1] << " "
	//	<< coefficients->values[2] << " "
	//	<< coefficients->values[3] << std::endl;
	////std::cerr << "Model inliers: " << inliers->indices.size() << std::endl;
	//std::cerr << " fMaxDist:" << fMaxDist << " Cloud Size:" << pCloud->size() << " Model inliers:" << inliers->indices.size() << " fInteriorPointScale:" << fInteriorPointScale << std::endl;
	//
	//填充点云数据
	//for (size_t i = 0; i < inliers->indices.size(); ++i)
	//{
	//	std::cerr << inliers->indices[i] << "    " << cloudTrayFoot->points[inliers->indices[i]].x << " "
	//		<< cloudTrayFoot->points[inliers->indices[i]].y << " "
	//		<< cloudTrayFoot->points[inliers->indices[i]].z << std::endl;
	//}

	//
	//float fAlpha = std::acos(coefficients->values[0]) / PI * 180;
	//float fBeta = std::acos(coefficients->values[1]) / PI * 180;
	//float fGamma = std::acos(coefficients->values[2]) / PI * 180;
	//float fDistance = coefficients->values[3];
	//std::cout << "fAlpha:" << fAlpha << " fBeta:" << fBeta << " fGamma:" << fGamma << " fDistance:" << fDistance << std::endl;


	//输出
	if (pcoefficients != NULL) {
		*pcoefficients = coefficients;
	}
	if (pCloudPlane != NULL) {
		ExtractIndicesCloud(pCloud, inliers, pCloudPlane);
	}
}

void AlgorithmPclPackage::PlaneCoefficients2Euler(const pcl::ModelCoefficients::Ptr& coefficients, float* pfAlpha, float* pfBeta, float* pfGamma, float* pfDist)
{
	float fAlpha = std::acos(coefficients->values[0]) / PI * 180;
	float fBeta = std::acos(coefficients->values[1]) / PI * 180;
	float fGamma = std::acos(coefficients->values[2]) / PI * 180;
	float fDistance = coefficients->values[3];
	//std::cout << "fAlpha:" << fAlpha << " fBeta:" << fBeta << " fGamma:" << fGamma << " fDistance:" << fDistance << std::endl;
	if (pfAlpha != NULL)*pfAlpha = fAlpha;
	if (pfBeta != NULL)*pfBeta = fBeta;
	if (pfGamma != NULL)*pfGamma = fGamma;
	if (pfDist != NULL)*pfDist = fDistance;
}

//计算平面点云位姿
// //计算平面点云位姿 利用最小包围盒 
void AlgorithmPclPackage::PlaneOBB(const PointCloudT::Ptr& pCloud, pcl::PointXYZ* pOBBPosition, Eigen::Matrix3f* pOBBRotationalMatrix, Eigen::Vector3f* pOBBSize, pcl::PointXYZ* pMassCenter)
{
	CloudOBB(pCloud, pOBBPosition, pOBBRotationalMatrix, pOBBSize, pMassCenter);
}
//点云最小包围盒
void AlgorithmPclPackage::CloudOBB(const PointCloudT::Ptr& pCloud, pcl::PointXYZ* pOBBPosition, Eigen::Matrix3f* pOBBRotationalMatrix, Eigen::Vector3f* pOBBSize, pcl::PointXYZ* pMassCenter)
{
	std::vector <float> moment_of_inertia;//惯性矩
	std::vector <float> eccentricity;//离心率
	//pcl::PointXYZ min_point_AABB;//坐标轴对齐包围盒
	//pcl::PointXYZ max_point_AABB;

	pcl::PointXYZ min_point_OBB;//有向包围盒
	pcl::PointXYZ max_point_OBB;
	pcl::PointXYZ position_OBB;//位置
	Eigen::Matrix3f rotational_matrix_OBB;//有向包围盒转动矩阵
	float major_value, middle_value, minor_value;//主要值 中值 次要值
	Eigen::Vector3f major_vector, middle_vector, minor_vector;//主要矢量 中值矢量 次要矢量
	Eigen::Vector3f mass_center;//重心

	//计算惯性矩描述子
	pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;//惯性与偏心率描述子类对象
	//feature_extractor.setAngleStep(2);
	//printf(" getAngleStep %f", feature_extractor.getAngleStep());//默认10？


	feature_extractor.setInputCloud(pCloud);
	feature_extractor.compute();//计算 耗时较多
	feature_extractor.getMomentOfInertia(moment_of_inertia);//获取惯性矩
	feature_extractor.getEccentricity(eccentricity);//获取离心率
	//feature_extractor.getAABB(min_point_AABB, max_point_AABB);//获取坐标轴对齐包围盒
	feature_extractor.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);//获取有向包围盒（不一定是最小包围盒）
	feature_extractor.getEigenValues(major_value, middle_value, minor_value);//3个特征值
	feature_extractor.getEigenVectors(major_vector, middle_vector, minor_vector);//3个特征向量
	feature_extractor.getMassCenter(mass_center);//点云中心坐标


	if (pOBBPosition != 0) {
		*pOBBPosition = position_OBB;
	}
	if (pOBBRotationalMatrix != 0) {
		*pOBBRotationalMatrix = rotational_matrix_OBB;
	}
	if (pMassCenter != 0) {
		pcl::PointXYZ MassCenter(mass_center.x(), mass_center.y(), mass_center.z());
		*pMassCenter = MassCenter;
	}
	if (pOBBSize != 0) {
		Eigen::Vector3f OBBSize(max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z);
		*pOBBSize = OBBSize;
	}

}


//点云AABB包围盒的面积
void AlgorithmPclPackage::CloudAABB(const PointCloudT::Ptr& pCloud, pcl::PointXYZ* maxpoint, pcl::PointXYZ* minPoint)
{
	std::vector <float> moment_of_inertia;//惯性矩
	std::vector <float> eccentricity;//离心率
	pcl::PointXYZ min_point_AABB;//坐标轴对齐包围盒
	pcl::PointXYZ max_point_AABB;

	float major_value, middle_value, minor_value;//主要值 中值 次要值
	Eigen::Vector3f major_vector, middle_vector, minor_vector;//主要矢量 中值矢量 次要矢量
	Eigen::Vector3f mass_center;//重心

	//计算惯性矩描述子
	pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;//惯性与偏心率描述子类对象

	feature_extractor.setInputCloud(pCloud);
	feature_extractor.compute();//计算 耗时较多
	feature_extractor.getMomentOfInertia(moment_of_inertia);//获取惯性矩
	feature_extractor.getEccentricity(eccentricity);//获取离心率
	feature_extractor.getAABB(min_point_AABB, max_point_AABB);//获取坐标轴对齐包围盒
	feature_extractor.getEigenValues(major_value, middle_value, minor_value);//3个特征值
	feature_extractor.getEigenVectors(major_vector, middle_vector, minor_vector);//3个特征向量
	feature_extractor.getMassCenter(mass_center);//点云中心坐标

	if (maxpoint != 0 && minPoint != 0) {
		*maxpoint = max_point_AABB;
		*minPoint = min_point_AABB;
	}
}

//点云AABB包围盒的面积
void AlgorithmPclPackage::CloudAABB(const PointCloudT::Ptr& pCloud, Eigen::Vector3f* pAABBSize, pcl::PointXYZ* pMassCenter)
{
	std::vector <float> moment_of_inertia;//惯性矩
	std::vector <float> eccentricity;//离心率
	pcl::PointXYZ min_point_AABB;//坐标轴对齐包围盒
	pcl::PointXYZ max_point_AABB;

	float major_value, middle_value, minor_value;//主要值 中值 次要值
	Eigen::Vector3f major_vector, middle_vector, minor_vector;//主要矢量 中值矢量 次要矢量
	Eigen::Vector3f mass_center;//重心

	//计算惯性矩描述子
	pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;//惯性与偏心率描述子类对象

	feature_extractor.setInputCloud(pCloud);
	feature_extractor.compute();//计算 耗时较多
	feature_extractor.getMomentOfInertia(moment_of_inertia);//获取惯性矩
	feature_extractor.getEccentricity(eccentricity);//获取离心率
	feature_extractor.getAABB(min_point_AABB, max_point_AABB);//获取坐标轴对齐包围盒
	feature_extractor.getEigenValues(major_value, middle_value, minor_value);//3个特征值
	feature_extractor.getEigenVectors(major_vector, middle_vector, minor_vector);//3个特征向量
	feature_extractor.getMassCenter(mass_center);//点云中心坐标

	if (pAABBSize != 0) {
		//假设不绕x和z旋转，只绕y轴旋转
		float dy = sqrt((max_point_AABB.z - min_point_AABB.z) * (max_point_AABB.z - min_point_AABB.z) + (max_point_AABB.y - min_point_AABB.y) * (max_point_AABB.y - min_point_AABB.y));
		//std::cout<< dy <<endl;
		//std::cout<< max_point_AABB.y - min_point_AABB.y <<endl;
		Eigen::Vector3f AABBSize(max_point_AABB.x - min_point_AABB.x, dy, max_point_AABB.z - min_point_AABB.z);
		*pAABBSize = AABBSize;
	}

	if (pMassCenter != 0) {
		pcl::PointXYZ MassCenter(mass_center.x(), mass_center.y(), mass_center.z());
		*pMassCenter = MassCenter;
	}
}

//估计边界 边界提取
int AlgorithmPclPackage::estimateBorders(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, float re, float reforn, pcl::PointCloud<pcl::PointXYZ>::Ptr* pcloud_boundary)
{
	pcl::PointCloud<pcl::Boundary> boundaries;//边界点云
	pcl::BoundaryEstimation<pcl::PointXYZ, pcl::Normal, pcl::Boundary> boundEst;//边界估计对象
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normEst;//法线估计对象
	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);//法线点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_boundary(new pcl::PointCloud<pcl::PointXYZ>);//边界点云坐标

	//估计法线
	normEst.setInputCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr(cloud));//设置输入点云
	normEst.setRadiusSearch(reforn);//设置法线查找半径 分辨率10倍左右 0.05
	normEst.compute(*normals);//计算法线

	//估计边界
	boundEst.setInputCloud(cloud);//设置点云
	boundEst.setInputNormals(normals);//设置法线
	boundEst.setRadiusSearch(re);//边界查找半径 分辨率10倍左右 0.05
	boundEst.setAngleThreshold(M_PI / 4);// 角度阈值 默认PI/2
	boundEst.setSearchMethod(pcl::search::KdTree<pcl::PointXYZ>::Ptr(new pcl::search::KdTree<pcl::PointXYZ>));//搜索模式
	boundEst.compute(boundaries);//计算

	//提取边界点
	for (int i = 0; i < cloud->points.size(); i++) {
		if (boundaries[i].boundary_point > 0) {
			cloud_boundary->push_back(cloud->points[i]);
		}
	}

	//显示边界
	if (0) {
		boost::shared_ptr<pcl::visualization::PCLVisualizer> MView(new pcl::visualization::PCLVisualizer("点云库PCL从入门到精通案例"));

		int v1(0);
		MView->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
		MView->setBackgroundColor(0.3, 0.3, 0.3, v1);
		MView->addText("Raw point clouds", 10, 10, "v1_text", v1);//原点云
		MView->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud", v1);
		MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "sample cloud", v1);
		MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud", v1);

		int v2(0);
		MView->createViewPort(0.5, 0.0, 1, 1.0, v2);
		MView->setBackgroundColor(0.5, 0.5, 0.5, v2);
		MView->addText("Boudary point clouds", 10, 10, "v2_text", v2);//边界点云
		MView->addPointCloud<pcl::PointXYZ>(cloud_boundary, "cloud_boundary", v2);
		MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "cloud_boundary", v2);
		MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud_boundary", v2);


		MView->addCoordinateSystem(1.0);
		MView->initCameraParameters();
		MView->spin();
	}

	*pcloud_boundary = cloud_boundary;

	return 0;
}


//凸包计算 凸包由最外围的点云构成
void AlgorithmPclPackage::ConvexHull(PointCloudT::Ptr& pCloud, PointCloudT::Ptr* pCloudConvexHull)
{
	if (pCloudConvexHull == NULL) {
		return;
	}
	// 计算质心凸包
	pcl::ConvexHull<pcl::PointXYZ> chull;
	chull.setInputCloud(pCloud);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull(new pcl::PointCloud<pcl::PointXYZ>);
	chull.reconstruct(*cloud_hull);
	*pCloudConvexHull = cloud_hull;


	//// 计算凸包中心点坐标
	//Eigen::Vector4f centroid;
	//pcl::compute3DCentroid(*cloud_hull, centroid);

	//// 计算凸包长宽高
	//Eigen::Vector4f min_pt, max_pt;
	//pcl::getMinMax3D(*cloud_hull, min_pt, max_pt);
	//float width = max_pt[0] - min_pt[0];
	//float height = max_pt[1] - min_pt[1];
	//float depth = max_pt[2] - min_pt[2];

	//// 计算凸包高度
	//float max_distance = 0;
	//for (auto point : cloud_hull->points) {
	//	float distance = pcl::squaredEuclideanDistance(point, centroid);
	//	max_distance = std::max(max_distance, distance);
	//}
	//float bounding_box_height = sqrt(max_distance);

	//// 构造类似OBB的数据结构
	//Eigen::Vector3f obb_center(centroid[0], centroid[1], centroid[2]);
	//Eigen::Vector3f obb_size(width, height, bounding_box_height);




}

//------------绘图-------------//
//绘制一维矢量
void AlgorithmPclPackage::plotvd(const std::vector<double>& vdY1, const std::string& name)
{
	//定义一个plottter类
	pcl::visualization::PCLPlotter* plotter = new pcl::visualization::PCLPlotter();
	std::vector<double> vdIndex;
	for (int ii = 0; ii < vdY1.size(); ii++) {
		vdIndex.push_back(ii);
	}
	//添加绘制的数据
	plotter->addPlotData(&vdIndex[0], &vdY1[0], vdY1.size(), name.c_str());
	//plotter->addPlotData(&vdIndex[0], &vdY2[0], vdY2.size(), "Y2");

	//设置Y轴区域
	//plotter->setYRange(-10, 1000);

	//一直显示
	//plotter->plot();

	//显示3秒
	//plotter->spinOnce(30000);
	plotter->spin();
	//plotter->clearPlots();
}

void AlgorithmPclPackage::plotvd(const std::vector<double>& vdY1, const std::string& Y1name, const std::vector<double>& vdY2, const std::string& Y2name)
{
	//定义一个plottter类
	pcl::visualization::PCLPlotter* plotter = new pcl::visualization::PCLPlotter();
	std::vector<double> vdIndex;
	for (int ii = 0; ii < vdY1.size(); ii++) {
		vdIndex.push_back(ii);
	}
	//添加绘制的数据
	plotter->addPlotData(&vdIndex[0], &vdY1[0], vdY1.size(), Y1name.c_str());
	plotter->addPlotData(&vdIndex[0], &vdY2[0], vdY1.size(), Y2name.c_str());
	//plotter->addPlotData(&vdIndex[0], &vdY2[0], vdY2.size(), "Y2");

	//设置Y轴区域
	//plotter->setYRange(-10, 1000);

	//一直显示
	//plotter->plot();

	//显示3秒
	//plotter->spinOnce(30000);
	plotter->spin();
	//plotter->clearPlots();
}
void AlgorithmPclPackage::plotvd(const std::vector<double>& vdY1, const std::string& Y1name, const std::vector<double>& vdY2, const std::string& Y2name, const std::vector<double>& vdY3, const std::string& Y3name)
{
	//定义一个plottter类
	pcl::visualization::PCLPlotter* plotter = new pcl::visualization::PCLPlotter();
	std::vector<double> vdIndex;
	for (int ii = 0; ii < vdY1.size(); ii++) {
		vdIndex.push_back(ii);
	}
	//添加绘制的数据
	plotter->addPlotData(&vdIndex[0], &vdY1[0], vdY1.size(), Y1name.c_str());
	plotter->addPlotData(&vdIndex[0], &vdY2[0], vdY1.size(), Y2name.c_str());
	plotter->addPlotData(&vdIndex[0], &vdY3[0], vdY1.size(), Y3name.c_str());
	//plotter->addPlotData(&vdIndex[0], &vdY2[0], vdY2.size(), "Y2");

	//设置Y轴区域
	//plotter->setYRange(-10, 1000);

	//一直显示
	//plotter->plot();

	//显示3秒
	//plotter->spinOnce(30000);
	plotter->spin();
	//plotter->clearPlots();
}

void AlgorithmPclPackage::plotvd(int count, ...)
{
	if (count <= 0) {
		return;
	}
	va_list arg_ptr;
	va_start(arg_ptr, count);
	//定义一个plottter类
	pcl::visualization::PCLPlotter* plotter = new pcl::visualization::PCLPlotter();
	std::vector<double> vdIndex;
	for (int i = 0; i < count; ++i) {
		const std::vector<double>& vdY1 = va_arg(arg_ptr, std::vector<double>);
		/*const std::string& name = va_arg(arg_ptr, std::string);*/
		const std::string& name(va_arg(arg_ptr, char*));
		//vdIndex = //genVector<double>(0, 1, vdY1.size());
		//添加绘制的数据
		plotter->addPlotData(&vdIndex[0], &vdY1[0], vdY1.size(), name.c_str());
	}

	plotter->spin();
}

void AlgorithmPclPackage::plotXY(int count, ...)
{
	if (count <= 0) {
		return;
	}
	va_list arg_ptr;
	va_start(arg_ptr, count);
	//定义一个plottter类
	pcl::visualization::PCLPlotter* plotter = new pcl::visualization::PCLPlotter();
	for (int i = 0; i < count; ++i) {
		const std::vector<double>& vdX1 = va_arg(arg_ptr, std::vector<double>);
		const std::vector<double>& vdY1 = va_arg(arg_ptr, std::vector<double>);
		/*const std::string& name = va_arg(arg_ptr, std::string);*/
		const std::string& name(va_arg(arg_ptr, char*));
		//添加绘制的数据
		plotter->addPlotData(&vdX1[0], &vdY1[0], vdY1.size(), name.c_str());
	}

	plotter->spin();
}

// 绘制二维矢量
void AlgorithmPclPackage::plotXY(const std::vector<double>& vdX1, const std::vector<double>& vdY1, const std::string& name)
{
	//定义一个plottter类
	pcl::visualization::PCLPlotter* plotter = new pcl::visualization::PCLPlotter();
	//添加绘制的数据
	plotter->addPlotData(&vdX1[0], &vdY1[0], vdY1.size(), name.c_str());
	//plotter->addPlotData(&vdIndex[0], &vdY2[0], vdY2.size(), "Y2");

	//设置Y轴区域
	//plotter->setYRange(-10, 1000);

	//一直显示
	//plotter->plot();

	//显示3秒
	//plotter->spinOnce(30000);
	plotter->spin();
	//plotter->clearPlots();
}
void AlgorithmPclPackage::plotXY(const std::vector<double>& vdX1, const std::vector<double>& vdY1, const std::string& Y1name, const std::vector<double>& vdX2, const std::vector<double>& vdY2, const std::string& Y2name)
{
	//定义一个plottter类
	pcl::visualization::PCLPlotter* plotter = new pcl::visualization::PCLPlotter();
	//添加绘制的数据
	plotter->addPlotData(&vdX1[0], &vdY1[0], vdY1.size(), Y1name.c_str());
	plotter->addPlotData(&vdX2[0], &vdY2[0], vdY2.size(), Y2name.c_str());
	//plotter->addPlotData(&vdIndex[0], &vdY2[0], vdY2.size(), "Y2");

	//设置Y轴区域
	//plotter->setYRange(-10, 1000);

	//一直显示
	//plotter->plot();

	//显示3秒
	//plotter->spinOnce(30000);
	plotter->spin();
	//plotter->clearPlots();
}

inline void AlgorithmPclPackage::viewPCL(const pcl::PointCloud<pcl::PointXYZ>& pointCloud)
{
	vtkOutputWindow::SetGlobalWarningDisplay(0);
	if (pointCloud.empty())
		return;

	pcl::visualization::PCLVisualizer cloudViewer("Cloud Viewer");
	cloudViewer.setShowFPS(false);
	cloudViewer.setBackgroundColor(0, 0, 0);
	cloudViewer.addPointCloud(pointCloud.makeShared());
	cloudViewer.addCoordinateSystem(0.01);
	cloudViewer.addText("Cloud Size: " + std::to_string(pointCloud.size()), 0, 25, 20, 1, 1, 1,
		"cloudSize");
	cloudViewer.addText("Press r/R to reset camera view point to center.", 0, 0, 16, 1, 1, 1,
		"help");
	cloudViewer.initCameraParameters();
	while (!cloudViewer.wasStopped()) {
		cloudViewer.spinOnce(20);
		//std::this_thread::sleep_for(std::chrono::milliseconds(100));
	}
}

inline void AlgorithmPclPackage::viewPCL(const pcl::PointCloud<pcl::PointXYZRGB>& colorPointCloud)
{
	vtkOutputWindow::SetGlobalWarningDisplay(0);
	if (colorPointCloud.empty())
		return;

	pcl::visualization::PCLVisualizer cloudViewer("Cloud Viewer");
	cloudViewer.setShowFPS(false);
	cloudViewer.setBackgroundColor(0, 0, 0);
	cloudViewer.addPointCloud(colorPointCloud.makeShared());
	cloudViewer.addCoordinateSystem(0.01);
	cloudViewer.addText("Cloud Size: " + std::to_string(colorPointCloud.size()), 0, 25, 20, 1, 1, 1,
		"cloudSize");
	cloudViewer.addText("Press r/R to reset camera view point to center.", 0, 0, 16, 1, 1, 1,
		"help");
	cloudViewer.initCameraParameters();
	while (!cloudViewer.wasStopped()) {
		cloudViewer.spinOnce(20);
		//std::this_thread::sleep_for(std::chrono::milliseconds(100));
	}
}
